#ifndef ROBOTHANSPLUGIN_H
#define ROBOTHANSPLUGIN_H

#include <QGenericPlugin>
#include <unistd.h>
#include <QLibrary>
#include "HR_Pro.h"
#include "robot_interface.h"


#define UnInitialize            0
#define Initialize              1
#define ElectricBoxDisconnect   2
#define ElectricBoxConnecting   3
#define EmergencyStopHandling   4
#define EmergencyStop           5
#define Blackouting48V          6
#define Blackout_48V            7
#define Electrifying48V         8
#define SaftyGuardErrorHandling 9
#define SaftyGuardError         10
#define SafetyGuardHandling     11
#define SaftyGuard              12
#define ControllerDisconnecting 13
#define ControllerDisconnect    14
#define ControllerConnecting    15
#define ControllerVersionError  16
#define EtherCATError           17
#define ControllerChecking      18
#define Reseting                19
#define RobotOutofSafeSpace     20
#define RobotCollisionStop      21
#define Error                   22
#define RobotEnabling           23
#define Disable                 24
#define Moving                  25
#define LongJogMoving           26
#define RobotStopping           27
#define RobotDisabling          28
#define RobotOpeningFreeDriver  29
#define RobotClosingFreeDriver  30
#define FreeDriver              31
#define RobotHolding            32
#define StandBy                 33
#define ScriptRunning           34
#define ScriptHoldHandling      35
#define ScriptHolding           36
#define ScriptStopping          37
#define ScriptStopped           38
#define HRAppDisconnected       39
#define HRAppError              40
#define RobotLoadIdentify       41
#define Braking                 42


const std::string coor_name = "Base";

class RobotHansPlugin : public QGenericPlugin ,public RobotInterface
{
    Q_OBJECT
#if QT_VERSION >= 0x050000
    Q_PLUGIN_METADATA(IID "robothansplugin.Plugin.RobotInterface")
    Q_INTERFACES(RobotInterface)
    Q_PLUGIN_METADATA(IID "org.qt-project.Qt.QGenericPluginFactoryInterface" FILE "Robot_hans.json")
#endif // QT_VERSION >= 0x050000

public:
    RobotHansPlugin(QObject *parent = 0);
    QObject* create(const QString& Name, const QString &Spec) override;
    //初始化和机器人通信相关参数
    //tcp是TCP名称
    bool Init(std::string Ip , uint16_t Port ,std::string Tcp = "TCP", uint32_t Boxid = 0 ,uint32_t RobotId = 0) override;
    //获取机器人扫描状态
    bool ReadScanState( bool  & State ) override;
    //获取拍照状态
    bool ReadTakePhotoState( bool & State ) override;
    //获取机器人状态
    bool GetRobotState( ROBOTSTATES &State ) override;
    //获取路径下标
    bool GetPathIndex( uint32_t &PathIndex )  override;
    //获取机器人位置，当前位置
    bool GetRobotPos( ROBOTPOS &PosNOw ) override;
    //将当前位置的欧拉角转为4元素
    bool GetQuaternion( double W , double X, double Y , double Z ) override;
    //移动机器人到位置
    bool MoveRobotTo( ROBOTPOS TargetPos ,
                      float MoveSpeed,
                      bool Congestion = true ) override;
    //控制机器人按路径移动
    bool RobotMovePath(std::vector<ROBOTPOS> Path,
                                        float MoveSpeed ,
                                        double SpeedRatio=0.5,
                                        double CntR=10,
                                        bool Congestion = true) override;
    //修改TCP
    bool SetTcpName( std::string TcpName ) override;
    //获取失败原因
    std::string GetLastError( void ) override;
    //清除错误消息
    void ClearError(void) override;
    //获取错误码
    int GetErrorCode( void );
private:
    //读取状态机状态
    int HR_ReadCurFSM(int &fsm);
    ///
    /// \brief judge_fsm状态判断，状态是否符合其中一个状态
    /// \param fsm：输入状态
    /// \param fsms：状态组
    /// \return true和其中一个状态符合  false：都不符合
    bool judge_fsm( int fsm, vector<int> fsms );
    int HR_ReadActJointPos( std::vector<double> & jointpos);
    int robot_set_override( double override  );
    int robot_enable( void );
    int wait_state( int Fsm );
    int wait_not_state( int Fsm );
    int HR_MoveL( ROBOTPOS pos , std::vector<double> ref_jointpos ,double Vel);
    int HR_MoveLByWayPoint( ROBOTPOS pos , std::vector<double> ref_jointpos ,double Vel);
    int HR_Electrify( void );
};

#endif // ROBOTHANSPLUGIN_H
